// #include <cv_bridge/cv_bridge.h>
// #include <image_transport/image_transport.h>
#include <iostream>
#include <math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
// #include <ros/ros.h>
//#include <windMill/result.h>
//windMill::result.msg msg;
using namespace std;
using namespace cv;
class ArmorDetection {
private:
  Mat frame, hsv, mask;
  Mat kernel1 = getStructuringElement(MORPH_RECT, Size(3, 3),
                                      Point(-1, -1)); //用于形态学操作去干扰
  Mat kernel2 =
      getStructuringElement(MORPH_RECT, Size(10, 10), Point(-1, -1)); // 用于膨胀
  Point2f currentCenter; //浮点数坐标
  Point2f lastCenter;
  vector<RotatedRect> minRects; //旋转矩形类向量
  int lost;
public:
  ArmorDetection();
  explicit ArmorDetection(Mat &input);
  void setInputImage(Mat input);
  void Pretreatment();
  Point2f GetArmorCenter();
//H*2   V/225  S/225
  int iLowH = 49;
  int iHighH = 93;
  int iLowS = 5;
  int iHighS = 95;
  int iLowV = 255;
  int iHighV = 255; //双阈值上下HSV界
private:
  void LostTarget();
  double Distance(Point2f, Point2f);
  double max(double, double);
  double min(double, double);
};

ArmorDetection::ArmorDetection() = default;
ArmorDetection::ArmorDetection(Mat &input) { frame = input; }
void ArmorDetection::setInputImage(Mat input) {
  frame = input;
  currentCenter.x = 0;
  currentCenter.y = 0;
}
//图像预处理
void ArmorDetection::Pretreatment() {
  Mat input;
  Point p, center;
  vector<vector<Point>> contours;           //用于 findContours函数，point每个点集构成一个轮廓
  vector<Vec4i> hireachy;                  // 4维int，用于 findContours函数
  vector<Rect> boundRect(contours.size()); // Rect类向量
  Point2f vertex[4];
  cvtColor(frame, hsv, COLOR_BGR2HSV);       // hsv色彩空间
  inRange(hsv,                            //双yuzhi 二值化
          Scalar(iLowH, iLowS, iLowV),    //下边界
          Scalar(iHighH, iHighS, iHighV), //上边界
          mask);                          // mask为输出图像
  morphologyEx(mask, mask, MORPH_OPEN, kernel1,Point(-1, -1)); // 形态学操作,开运算，先腐蚀再膨胀，清楚背景小的亮干扰
  dilate(mask, mask, kernel2, Point(-1, -1), 1); //膨胀
   Canny(mask, mask, 3, 9, 3);                    //轮廓增强     393
  imshow("mask", mask);
  findContours(mask, contours, hireachy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
  //hireachy向量内每一个元素=4个int     分别：后一轮廓、前一轮廓、…………
  // CV_RETR_EXTERNAL  只检测最外围轮廓
  //CHAIN_APPROX_SIMPLE  只保存轮廓的拐点信息->contours              矩形
  //筛选，去除一部分矩形
  for (int i = 0; i < contours.size(); ++i) {
    RotatedRect minRect = minAreaRect(Mat(contours[i]));     //包住contours[i]拐点的最小旋转矩形
    minRect.points(vertex);                                                                  //将坐标放到vertex中
    if (minRect.size.width > minRect.size.height) {              //矩形摆正
      minRect.angle += 90;
      float t = minRect.size.width;
      minRect.size.width = minRect.size.height;
      minRect.size.height = t;
    }
    if ((minRect.size.width * 10 > minRect.size.height) &&
        (minRect.size.width * 1 < minRect.size.height) &&
        (abs(minRect.angle) < 30)) {
      minRects.push_back(minRect);        //符合长宽比的矩形留下
    }
    for (int l = 0; l < 4; l++) {
      line(frame, vertex[l], vertex[(l + 1) % 4], Scalar(0, 255, 255), 5);     //2
    }        //起点  终点  线段颜色  宽度  高斯滤波
   /* line(frame, Point(frame.cols / 2 - 15, frame.rows / 2),
         Point(frame.cols / 2 + 15, frame.rows / 2), Scalar(0, 255, 255), 5);
    line(frame, Point(frame.cols / 2, frame.rows / 2 - 15),
         Point(frame.cols / 2, frame.rows / 2 + 15), Scalar(0, 255, 255), 5);
    circle(frame, Point(frame.cols / 2, frame.rows / 2), 4, Scalar(0, 0, 255),
           -1);         //画面中心*/
  }
}
Point2f ArmorDetection::GetArmorCenter() {
  //遍历所有矩形，两两组合
  RotatedRect leftRect, rightRect;
  vector<int *> reliability;
  double area[2], distance, height;
  if (minRects.size() < 2) {            //少于两个灯柱
    LostTarget();
    return currentCenter;
  }
  for (int i = 0; i < minRects.size(); ++i) {
    for (int j = i + 1; j < minRects.size(); ++j) {
      int level = 0;
      int temp[3];
      leftRect = minRects[i];
      rightRect = minRects[j];
      //判断  评分
      if (leftRect.angle == rightRect.angle) {
        level += 10;
      } else if (abs(leftRect.angle - rightRect.angle) < 5) {
        level += 8;
      } else if (abs(leftRect.angle - rightRect.angle) < 10) {
        level += 6;
      } else if (abs(leftRect.angle - rightRect.angle) < 30) {
        level += 4;
      } else if (abs(leftRect.angle - rightRect.angle) < 90) {
        level += 1;
      } else {
        break;
      }
      area[0] = leftRect.size.width * leftRect.size.height;
      area[1] = rightRect.size.width * rightRect.size.height;
      if (area[0] == area[1]) {
        level += 10;
      } else if (min(area[0], area[1]) * 1.5 > max(area[0], area[1])) {
        level += 8;
      } else if (min(area[0], area[1]) * 2 > max(area[0], area[1])) {
        level += 6;
      } else if (min(area[0], area[1]) * 3 > max(area[0], area[1])) {
        level += 4;
      } else if (min(area[0], area[1]) * 4 > max(area[0], area[1])) {
        level += 1;
      } else {
        break;
      }
      double half_height = (leftRect.size.height + rightRect.size.height) / 4;
      if (leftRect.center.y == rightRect.center.y) {
        level += 10;
      } else if (abs(leftRect.center.y - rightRect.center.y) <
                 0.2 * half_height) {
        level += 8;
      } else if (abs(leftRect.center.y - rightRect.center.y) <
                 0.4 * half_height) {
        level += 6;
      } else if (abs(leftRect.center.y - rightRect.center.y) <
                 0.8 * half_height) {
        level += 4;
      } else if (abs(leftRect.center.y - rightRect.center.y) < half_height) {
        level += 1;
      } else {
        break;
      }
      distance = Distance(leftRect.center, rightRect.center);
      height = (leftRect.size.height + rightRect.size.height) / 2;
      if (distance != 0 && distance > height) {
        if (distance < 1.5 * height) {
          level += 6;
        } else if (distance < 1.8 * height) {
          level += 4;
        } else if (distance < 2.4 * height) {
          level += 2;
        } else if (distance < 10 * height) {
          level += 1;
        } else {
          break;
        }
      }
      temp[0] = i;
      temp[1] = j;
      temp[2] = level;
      reliability.push_back(temp);
    }
  }
  if (reliability.empty()) {
    LostTarget();
    return currentCenter;
  } else {
    int maxLevel = 0, index = 0;
    for (int k = 0; k < reliability.size(); ++k) {
      if (reliability[k][2] > maxLevel) {
        maxLevel = reliability[k][2];
        index = k;
      }
    }
    currentCenter.x = (minRects[reliability[index][0]].center.x +
                       minRects[reliability[index][1]].center.x) /
                      2;
    currentCenter.y = (minRects[reliability[index][0]].center.y +
                       minRects[reliability[index][1]].center.y) /
                      2;
    //与上一次的结果对比
    if (lastCenter.x == 0 && lastCenter.y == 0) {
      lastCenter = currentCenter;
      lost = 0;
    } else {
      double difference = Distance(currentCenter, lastCenter);
      if (difference > 300) {     //300
        LostTarget();
        return currentCenter;
      }
    }
    line(frame, Point(currentCenter.x - 10, currentCenter.y - 10),
         Point(currentCenter.x + 10, currentCenter.y + 10), Scalar(0, 0, 255),
       10);
    line(frame, Point(currentCenter.x + 10, currentCenter.y - 10),
         Point(currentCenter.x - 10, currentCenter.y + 10), Scalar(0, 0, 255),
         10);
       //标出识别装甲板中心
    imshow("frame", frame);
    return currentCenter;
  }
}
void ArmorDetection::LostTarget() {
  lost++;
  if (lost < 3) {                                                        //3
    currentCenter = lastCenter;
  } else {
    currentCenter = Point2f(-1, -1);                //0，0
    lastCenter = Point2f(0, 0);
  }
}
double ArmorDetection::Distance(Point2f a, Point2f b) {
  return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}
double ArmorDetection::max(double first, double second) {
  return first > second ? first : second;
}
double ArmorDetection::min(double first, double second) {
  return first < second ? first : second;
}
ArmorDetection *armor = new ArmorDetection();        //创建敌方
Point2f center;
int main(int argc, char **argv) {
//   ros::init(argc, argv, "image_publisher");
//   ros::NodeHandle nh;
  Mat frame;
  int frame_no;   //帧数
  VideoCapture capture("test.avi");
  if (!capture.isOpened()) {
    printf("error\n");
    return -1;
  }
  namedWindow("frame", WINDOW_AUTOSIZE);
  namedWindow("mask", WINDOW_AUTOSIZE);
 // namedWindow("Control", CV_WINDOW_AUTOSIZE);
  while (capture.read(frame)) //读取当前帧
  {
    armor->setInputImage(frame);
    armor->Pretreatment();
    center = armor->GetArmorCenter();
    frame_no=capture.get(CAP_PROP_APERTURE);
    cout << frame_no<<"[INFO] x = " << center.x
         << "    y = " << center.y  << endl;
    char c = waitKey(10);
    if (c == 27) //"Esc"退出
    {
      break;
    }
  }
  capture.release(); //释放视频内存
  waitKey(0);
  return 0;
}